import socket
import json
import time


def connectETController(ip, port=8055):
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    try:
        sock.connect((ip, port))
        return (True, sock)
    except Exception as e:
        sock.close()
        return (False, None)


def disconnectETController(sock):
    if sock:
        sock.close()
        sock = None
    else:
        sock = None


def sendCMD(sock, cmd, params=None, id=1):
    if not params:
        params = []
    else:
        params = json.dumps(params)

    sendStr = (
        '{{"method":"{0}","params":{1},"jsonrpc":"2.0","id":{2}}}'.format(
            cmd, params, id
        )
        + "\n"
    )
    try:
        sock.sendall(bytes(sendStr, "utf-8"))
        ret = sock.recv(1024)
        jdata = json.loads(str(ret, "utf-8"))
        if "result" in jdata.keys():
            return (True, json.loads(jdata["result"]), jdata["id"])
        elif "error" in jdata.keys():
            return (False, jdata["error"], jdata["id"])
        else:
            return (False, None, None)
    except Exception as e:
        return (False, None, None)


if __name__ == "__main__":
    # 机器人 IP 地址
    robot_ip = "192.168.1.233"
    conSuc, sock = connectETController(robot_ip)
    print(conSuc)
    if conSuc:
        # 清除警报
        ret, result, id = sendCMD(sock, "clearAlarm")
        print("清除报警")
        print("ret = ", ret, " ", "id = ", id)
        if ret:
            print("result = ", result)
            time.sleep(1)

        # 获取同步状态
        ret, result, id = sendCMD(sock, "getMotorStatus")
        print("获取同步状态")
        print("ret = ", ret, " ", "id = ", id)
        if ret:
            print("result = ", result)
            if result != 1:
                # 同步
                ret1, result1, id = sendCMD(sock, "syncMotorStatus")
                print("同步")
                print("ret = ", ret1, " ", "id = ", id)
                if ret1:
                    print("result = ", result1)
                    time.sleep(0.5)
                else:
                    print(" err_msg = ", result1[" message "])
        else:
            print(" err_msg = ", result[" message "])
        # 打开伺服
        ret, result, id = sendCMD(sock, "set_servo_status", {"status": 1})
        print("打开伺服")
        print("ret = ", ret, " ", "id = ", id)
        if ret:
            print("result = ", result)
            time.sleep(1)
        else:
            print(" err_msg = ", result[" message "])
        # 获取伺服状态
        ret, result, id = sendCMD(sock, "getServoStatus")
        print("ret = ", ret, " ", "id = ", id)
        if ret:
            print("result = ", result)
        else:
            print("err_msg = ", result["message"])
    else:
        print("连接失败")

    disconnectETController(sock)
